

#include "funcdef.h"


#define MSG_LEN		1
#define SERIAL_BUFFER_SIZE 		1024

#pragma DATA_ALIGN(Term_RxBuffer, SERIAL_BUFFER_SIZE);
static Int8 Term_RxBuffer[SERIAL_BUFFER_SIZE];

#pragma DATA_ALIGN(Term_TxBuffer, SERIAL_BUFFER_SIZE);
static Int8 Term_TxBuffer[SERIAL_BUFFER_SIZE];

const char MENU[] =
"\n\r--------- NAVCON Configuration ---------\n\r\
Set BMA180 Scale\n\r\
a)\t- 1.0 g\n\r\
b)\t- 1.5 g\n\r\
c)\t- 2.0 g\n\r\
d)\t- 3.0 g\n\r\
e)\t- 4.0 g\n\r\
f)\t- 8.0 g\n\r\
g)\t- 16.0 g\n\r\
Set HMC5883L Scale\n\r\
h)\t- 0.88 Ga\n\r\
i)\t- 1.3 Ga\n\r\
j)\t- 1.9 Ga\n\r\
k)\t- 2.5 Ga\n\r\
l)\t- 4.0 Ga\n\r\
m)\t- 4.7 Ga\n\r\
n)\t- 5.6 Ga\n\r\
o)\t- 8.1 Ga\n\r\
p)\t- Set TXT RAW DATALOG\n\r\
q)\t- Set BIN RAW DATALOG\n\r\
r)\t- Set FULL_SET_SCALE DATALOG\n\r\
t)\t- Set Benchmark DATALOG\n\r\
s)\t- Start/Stop DATALOG\n\r\
v)\t- Software Version\n\r";


static int nPrint_MENU(TSK_Ctrl_b *tsk);
static char dec_msg(char* buf,size_t len);
static int serial_printf_str(TSK_Ctrl_b *tsk, char* str);

void serial_terminal(void)
{
	int error = 0;
	size_t len = 0;
	char cmd = 0;
	int MSG_SEND_REQ = 0;
	Msg message;
	QUE_Handle destination;
	SEM_Handle q_sem;
	TSK_Ctrl_b *self = &(SCB.TCB[TSK_ctrl_term]);
	q_sem = SEM_create(0, &SEM_ATTRS);
	message = MEM_alloc(0, sizeof(MsgObj), 0);
	// check out where the memory segment locates.
	LOG_printf(self->trace," message addr = %x", MEM_getBaseAddress(0));
	message->sender = TSK_ctrl_term;
	message->receipt = self->queue;

	for(;;){

		SEM_reset(q_sem, 0); // reset semaphore value to 0
		TSK_sleep(100);
		if(FALSE == SCB.TSK_ENA[TSK_datalog]){
			error = nPrint_MENU(self);
			if(error){
				//DEBUG
				LOG_printf(self->trace,"fail to print menu: %d",error);
			}
		}
		//Block waiting on UART2 RX
		//read message from Ground station for MSG_LEN bytes and return error
		len = MSG_LEN;
		error = GIO_submit(self->inhd,IOM_READ,Term_RxBuffer,&len,NULL);
		if(error){
			//DEBUG
			LOG_printf(self->trace, "serial_read failed: %d\n", error);
		}else{
			//error = msg_check(Term_RxBuffer, MSG_LEN);//check msg before proceed
			if(error){
				//LOG_printf(&Term_trace, "msg_check failed: %d\n", error);
			}else{
				cmd = dec_msg(Term_RxBuffer, MSG_LEN);//decode message
				//printf("Rev cmd: %c\n",cmd);
				if(TRUE == SCB.TSK_ENA[TSK_datalog]){
					//if datalog is active, only allow disable datalog cmd!!!!
					switch(cmd){
					case 's':
						SCB.TSK_ENA[TSK_datalog] = FALSE;
						break;
					default:
						break;
					}

				}else{
					MSG_SEND_REQ = TRUE;
					switch(cmd){
					/* Accelerometer */
					case 'a':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_1G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 1.0 g ...");
						break;
					case 'b':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_1_5G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 1.5 g ...");
						break;
					case 'c':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_2G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 2.0 g ...");
						break;
					case 'd':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_3G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 3.0 g ...");
						break;
					case 'e':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_4G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 4.0 g ...");
						break;
					case 'f':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_8G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 8.0 g ...");
						break;
					case 'g':
						message->cmd_1 = ACC_SET_RANGE;
						message->cmd_2 = BMA180_16G;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_BMA180].queue;
						error = serial_printf_str(self, "Setting Acc range to 16.0 g ...");
						break;

					/* Magenotometer */
					case 'h':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_0P88GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 0.88 g ...");
						break;
					case 'i':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_1P3GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 1.3 g ...");
						break;
					case 'j':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_1P9GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 1.9 g ...");
						break;
					case 'k':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_2P5GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 2.5 g ...");
						break;
					case 'l':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_4P0GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 4.0 g ...");
						break;
					case 'm':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_4P7GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 4.7 g ...");
						break;
					case 'n':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_5P6GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 5.6 g ...");
						break;
					case 'o':
						message->cmd_1 = MAG_SET_RANGE_CMD;
						message->cmd_2 = MAG_8P1GA;
						message->sem = q_sem;
						destination = SCB.TCB[TSK_HMC5883L].queue;
						error = serial_printf_str(self, "Setting Mag range to 8.1 g ...");
						break;

					case 'p':
						DATA_LOG_TYPE = TXT_DATA_LOG;
						serial_printf_str(self, "Datalog: TXT\n");
						MSG_SEND_REQ = FALSE;
						break;
					case 'q':
						DATA_LOG_TYPE = BIN_DATA_LOG;
						serial_printf_str(self, "Datalog: Binary\n");
						MSG_SEND_REQ = FALSE;
						break;
					case 'r':
						DATA_LOG_TYPE = FULL_SET_SCALE_DATA_LOG;
						serial_printf_str(self, "Datalog: full scale\n");
						MSG_SEND_REQ = FALSE;
						break;
					case 't':
						DATA_LOG_TYPE = SYSTEM_PROFILING;
						serial_printf_str(self, "Datalog: SYSTEM_PROFILING\n");
						MSG_SEND_REQ = FALSE;
						break;
					case 'u':
						DATA_LOG_TYPE = GPS_DATA_LOG;
						serial_printf_str(self, "Datalog: GPS_DATA_LOG\n");
						MSG_SEND_REQ = FALSE;
						break;
					case 's':
						//enable/disable data logging
						if(TRUE == SCB.TSK_ENA[TSK_datalog]){
							SCB.TSK_ENA[TSK_datalog] = FALSE;
						}else{
							SCB.TSK_ENA[TSK_datalog] = TRUE;
						}
						MSG_SEND_REQ = FALSE;
						break;
					case 'v':
#ifdef _DEBUG
						serial_printf_str(self,"In Debug Version\n");
#else
						serial_printf_str(self,"In Release Version 2.0\n");
#endif
						MSG_SEND_REQ = FALSE;
						break;
					default:
						MSG_SEND_REQ = FALSE;
						break;

					}
					if(TRUE == MSG_SEND_REQ){
						//send message
						QUE_put(destination, message);
						//wait for reply
						if(SEM_pend(q_sem,SYS_TIME_OUT)){
							//get reply message
							message = QUE_get(self->queue);
							if(message->status){
								//fail
								error = serial_printf_str(self, "FAIL\n");
								//printf("set failed : %d\n",message->status);
							}else{
								//success
								error = serial_printf_str(self, "SUCCESS\n");
								//printf("set successfully\n");
							}
						}else{
							//timeout expires
							error = serial_printf_str(self, "Timeout!\n");
							//DEBUG
							LOG_printf(self->trace, " Timeout!\n");

						}
						MSG_SEND_REQ = FALSE;
					}
				}
			}
		}

		TSK_yield();
	}
}


static int nPrint_MENU(TSK_Ctrl_b *tsk){
	int status= 0;
	size_t len = strlen(MENU);

	memcpy(Term_TxBuffer, MENU, len);
	//LOG_printf(tsk->trace, "MENU LENGTH: %d\n", len);
	if(SEM_pendBinary(&UART2_OUT_SEM,SYS_TIME_OUT)){
		status = GIO_submit(tsk->outhd,IOM_WRITE, Term_TxBuffer, &len, NULL);
		SEM_postBinary(&UART2_OUT_SEM);
	}else{
		//time out
		//DEBUG
		LOG_printf(tsk->trace, "UART2 Timeout!\n");
		status = -1;
	}
	return status;
}


static int serial_printf_str(TSK_Ctrl_b *tsk, char* str){
	size_t len = 0;
	int status= 0;

	len =  strlen(str);
	memcpy(Term_TxBuffer,str,len);
	if(SEM_pendBinary(&UART2_OUT_SEM,SYS_TIME_OUT)){
		status = GIO_submit(tsk->outhd,IOM_WRITE, Term_TxBuffer, &len, NULL);
		SEM_postBinary(&UART2_OUT_SEM);
	}else{
		//time out
		//DEBUG
		LOG_printf(tsk->trace, "UART2 Timeout!\n");
		status = -1;
	}
    return status;
}



static char dec_msg(char* buf,size_t len){
//	int i = 0;
/*	int cmd_t = 0;
	int temp = 0;

	for (i=0;i<len;i++){
		if(buf[i]>='0' && buf[i]<='9'){
			temp = buf[i] - '0';
			cmd_t = cmd_t *10 + temp;
		}
	}
	return cmd_t;*/
	return buf[0];

}



